/*********************************************************************************************************
** 文件创建日期: 2023 年 04 月 06 日
**
** 描        述: 电机控制
*********************************************************************************************************/
#include <stdio.h>
#include "securec.h"
#include "sys/types.h"
#include "sys/stat.h"
#include "sys/ioctl.h"
#include <unistd.h>
#include <fcntl.h>
#include "private.h"
#include "motorControl.h"

/**
 * 初始化电机控制器
 * 配置为modbus控制
 */
MotorControl* motorInit()
{
    int ret = 0;
    MotorControl *mc = NULL;
    mc = (MotorControl*)malloc(sizeof(MotorControl));
    if (mc == NULL){
        printf("error:  malloc motorControl CB error \n");
        return NULL;
    }
    
    mc->ctx = modbus_new_rtu(MODEBUS_DEV, 19200, 'N', 8, 1);
    if(modbus_connect(mc->ctx) == -1) {
        printf("error: Connection failed\n");
        modbus_free(mc->ctx);
        goto error1;
    }
    if(modbus_set_slave(mc->ctx, 1) == 0){
        printf("modbus_set_slave succeed\n");
    }

    ret = modbus_set_response_timeout(mc->ctx, 1, 0);
    if(ret != 0){
        printf("error: Velocity given selection failed\n");
        modbus_free(mc->ctx);
        goto error1;
    }

    ret = modbus_write_register(mc->ctx, RUN_INSTRUCTION_SELECTION, 2);//使用modbus通信指令控制
    if(ret == -1){
        printf("error: Run instruction selection failed\n");
        modbus_free(mc->ctx);
        goto error1;
    }

    ret = modbus_write_register(mc->ctx, VELOCITY_GIVEN_SELECTION, 3);//使用modbus通信速度控制
    if(ret == -1){
        printf("error: Velocity given selection failed\n");
        modbus_free(mc->ctx);
        goto error1;
    }
    return mc;
error1:
    free(mc);
    return NULL;
}

int motorRelease(MotorControl *mc){
    int ret = 0;
    ret = modbus_write_register(mc->ctx, OPERATING_MODE, 5);
    if(ret == -1){
        printf("error: Set rotation mode failed\n");
        return -1;
    }
    modbus_free(mc->ctx);
    free(mc);
}
/**
 * 设置转速 通讯设定转速 (0~3000(单位: 1RPM))
 */
int motorSetSpeed(MotorControl *mc, unsigned short int speed)
{
    int ret = 0;
    if(speed > MAXIMUM_SPEED){
        printf("error: overspeed \n");
        return -1;
    }
    ret = modbus_write_register(mc->ctx, SPEED_SETTING, speed);
    if(ret == -1){
        printf("error: Set rotation speed failed\n");
        return -1;
    }
    return 0;
}
unsigned short int  motorGetSpeed(MotorControl *mc)
{
    int ret = 0;
    unsigned short int speed = 0;

    ret = modbus_read_registers(mc->ctx, SPEED_SETTING, 2, &speed);
    if(ret == -1){
        printf("error: Set rotation speed failed\n");
        return -1;
    }
    // printf("speed :%d\n", speed);
    return speed;
}
/**
 * 设置操作模式
 * mode = 1  正传运行
 * mode = 2  反转运行
 * mode = 3  正转点动
 * mode = 4  反转点动
 * mode = 5  停机
 */
int motorSetModel(MotorControl *mc, unsigned short int mode)
{
    int ret = 0;
    ret = modbus_write_register(mc->ctx, OPERATING_MODE, mode);
    if(ret == -1){
        printf("error: Set rotation mode failed\n");
        return -1;
    }
    return 0;
}
/**
 * 读取状态
 * 0001H:正转运行中
 * 0002H:反转运行中
 * 0003H:驱动器停机中
 * 0004H:驱动器故障中
 * 0005H:驱动器OFF状态
 * 0006H:电子刹车状态
 */
unsigned short int motorGetModel(MotorControl *mc)
{
    int ret = 0;
    unsigned short int value = 0;
    ret = modbus_read_registers(mc->ctx, OPERATING_MODE, 1, &value);
    if(ret == -1){
        printf("error: Set rotation mode failed\n");
        return -1;
    }

    return value;
}